package org.nashua.tt151.systems;

import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Talon;
import edu.wpi.first.wpilibj.templates.Dash;
import java.util.Timer;
import java.util.TimerTask;
import org.nashua.tt151.libraries.Controller.DualAction;

/**
 * 2014 Drive Train Control System
 *
 * @author Kareem El-Faramawi
 */
public class DriveTrain extends Subsystem {
	private static DriveTrain INSTANCE;
	private static final Talon r1 = new Talon( 1, 1 );
	private static final Talon r2 = new Talon( 1, 2 );
	private static final Talon l1 = new Talon( 1, 3 );
	private static final Talon l2 = new Talon( 1, 4 );
	private static final Encoder leftEncoder = new Encoder( 1, 1, 1, 2, true );
	private static final Encoder rightEncoder = new Encoder( 1, 3, 1, 4 );

	private final double DISTANCE_PER_TICK = 4.0 * Math.PI / 256.0;

	private double mult = 0.75;
	private final double ADJUSTMENT_FACTOR = 0.0025;
	private final double MAX_ADJUSTMENT = 0.1;
	private volatile double rightAdjustment = 0.0;
	private volatile double leftAdjustment = 0.0;

	private boolean autoMove = false;
	public boolean enabled = false;

	DualAction driver = null;

	private DriveTrain() {
		new Timer().scheduleAtFixedRate( new TimerTask() {
			double jValLeft;
			double jValRight;
			double tickLeft;
			double tickRight;
			double jRatio;
			double error;

			public void run() {
				if ( driver != null && enabled ) {
					try {
						//Get joystick values
						jValLeft = driver.getLeftY() * mult;
						jValRight = driver.getRightY() * mult;
						//Reset encoder ticks
						tickLeft = leftEncoder.get();
						tickRight = rightEncoder.get();
						//Let the wheels run for a small dt
						Thread.sleep( 5 );
						//Get the actual number of rotations during the dt
						tickLeft = leftEncoder.get() - tickLeft;
						tickRight = rightEncoder.get() - tickRight;
						//Only make adjustments if the joysticks pass a small deadzone
						if ( Math.abs( jValRight ) >= 0.05 && Math.abs( jValLeft ) >= 0.05 ) {
							//Use the faster side as a reference for the adjustment
							if ( Math.abs( jValRight ) > Math.abs( jValLeft ) ) {
								jRatio = jValRight / jValLeft;
								error = tickLeft - ( tickRight / jRatio );
								leftAdjustment += error * ADJUSTMENT_FACTOR;

								if ( Math.abs( leftAdjustment ) > MAX_ADJUSTMENT ) {
									if ( leftAdjustment < -MAX_ADJUSTMENT ) {
										rightAdjustment += -MAX_ADJUSTMENT - leftAdjustment;
									} else {
										rightAdjustment += MAX_ADJUSTMENT - leftAdjustment;
									}
								}
							} else if ( Math.abs( tickLeft ) > Math.abs( tickRight ) ) {
								jRatio = jValLeft / jValRight;
								error = tickRight - ( tickLeft / jRatio );
								rightAdjustment += error * ADJUSTMENT_FACTOR;

								if ( Math.abs( rightAdjustment ) > MAX_ADJUSTMENT ) {
									if ( rightAdjustment < -MAX_ADJUSTMENT ) {
										leftAdjustment += -MAX_ADJUSTMENT - rightAdjustment;
									} else {
										leftAdjustment += MAX_ADJUSTMENT - rightAdjustment;
									}
								}
							}
						} else {
							// Reset the adjustments if the robot is not moving
							rightAdjustment = 0.0;
							leftAdjustment = 0.0;
						}
						leftAdjustment = clamp( -MAX_ADJUSTMENT, MAX_ADJUSTMENT, leftAdjustment );
						rightAdjustment = clamp( -MAX_ADJUSTMENT, MAX_ADJUSTMENT, rightAdjustment );
					} catch ( InterruptedException ex ) {
						System.err.println( ex.getMessage() );
					}

				}
			}
		}, 1, 1 );
	}

	public static DriveTrain getInstance() {
		if ( INSTANCE == null ) {
			INSTANCE = new DriveTrain();
		}
		return INSTANCE;
	}

	public void init() {
		autoMove = false;
		leftEncoder.start();
		leftEncoder.reset();
		leftEncoder.setDistancePerPulse( DISTANCE_PER_TICK );
		rightEncoder.start();
		rightEncoder.reset();
		rightEncoder.setDistancePerPulse( DISTANCE_PER_TICK );
		set( 0, 0 );
	}

	private long lastZero = System.currentTimeMillis();

	public void operatorControl( DualAction driver, DualAction shooter ) {
		if ( driver != this.driver ) {
			this.driver = driver;
		}

//		if ( autoMove ) {
//			double turnX = Dash.camTurnHoriz;
//			set( turnX, turnX );
//			if ( turnX == 0.0 ) {
//				if ( System.currentTimeMillis() - lastZero > 1000 ) {
		//toggleAutoMove();
//					double distOff = Camera.getInstance().getDist() - 5.0;
//					if(Math.abs(distOff) > 0.2) {
//						if (distOff > 0)  {
//							set( -0.2, 0.2);
//						} else {
//							set(0.2, -0.2);
//						}
//					} else {
//					toggleAutoMove();
//					}
//				}
//			} else {
//				lastZero = System.currentTimeMillis();
//			}
//		} else {
		mult = 0.75;
		if ( driver.getRawButton( DualAction.Button.LEFT_TRIGGER ) ) {
			mult = 0.25;
		}
		set( driver.getRightY() * mult, -driver.getLeftY() * mult );
//		}

//		if ( driver.wasReleased( DualAction.Button.Y ) ) {
//			toggleAutoMove();
//		}
	}

	public void set( double right, double left ) {
		r1.set( clamp( -1.0, 1.0, right + rightAdjustment ) );
		r2.set( clamp( -1.0, 1.0, right + rightAdjustment ) );
		l1.set( clamp( -1.0, 1.0, left - leftAdjustment ) );
		l2.set( clamp( -1.0, 1.0, left - leftAdjustment ) );
	}

	public void setAutoMove( boolean autoMove ) {
		this.autoMove = autoMove;
	}

	private double clamp( double min, double max, double val ) {
		return Math.min( max, Math.max( min, val ) );
	}
}
